﻿using System;
using System.Collections.Generic;
using System.Text;
using LocalizationTest.Tools;

namespace LocalizationTest
{
  class MBLSimpleGridRefinement : SelfLocator
  {
    List<FunctionWorldState> functionTemplate;
    WorldState minimum;
    WorldState maximum;
    WorldState step;

    WorldState lastBestWorldstate = null;

    public override WorldState Execute(Experiment experiment)
    {
      if (experiment.actionAndObservationHistory.history.Count == 0) return null;
      List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces = experiment.actionAndObservationHistory.GetSelectedRelativePlaces(experiment.simulatedWorldState);
      if (listOfOldPlaces.Count == 0) return null;

      minimum = Globals.GetMinimum(experiment.simulatedWorldState);
      maximum = Globals.GetMaximum(experiment.simulatedWorldState);
      step = Globals.GetStep(experiment.simulatedWorldState);

      /*
      if (listOfOldPlaces.Count > 10 && lastBestWorldstate != null)
      {
        ResetRangeAndStepWidth(lastBestWorldstate, 0.5);
        step = Globals.GetStep(experiment.simulatedWorldState);
        ResetRangeAndStepWidth(lastBestWorldstate, 0.5);
      }
      */

      functionTemplate = experiment.simulatedWorldState.CreateListOfWorldStates(minimum, maximum, step);

      int numberOfRefinements = 1;
      if (DebugKeyTool.IsActive("MBLSimpleGridRefinement: multiple refinements")) numberOfRefinements = 6;

      double minimalValue;
      double maximalValue;
      WorldState positionOfMinimalValue = null;
      WorldState positionOfMaximalValue;

      for (int i = 0; i < numberOfRefinements; i++)
      {
        List<FunctionWorldState> newFunction;

        FindMinimum(listOfOldPlaces,
          functionTemplate, out newFunction,
          out minimalValue, out positionOfMinimalValue,
          out maximalValue, out positionOfMaximalValue,
          experiment);

        DrawFunction("world:MBLSimpleGridRefinement:likelihood function:" + i.ToString(), newFunction, minimalValue, maximalValue);
        positionOfMinimalValue.DrawHighlighted("world:MBLSimpleGridRefinement:refinement step:" + i.ToString(), System.Drawing.Color.Brown);
        ResetRangeAndStepWidth(positionOfMinimalValue, 0.5);
        functionTemplate = experiment.simulatedWorldState.CreateListOfWorldStates(minimum, maximum, step);
      }
      lastBestWorldstate = positionOfMinimalValue;
      return positionOfMinimalValue;

      /*
      WorldState result;
      result = MBLTools.DoGradientDescent2(positionOfMinimalValue, experiment);
      return result;
      */
    }

    private void FindMinimum(
    List<ActionAndObservationHistory.RelativePlaceWithMeasurement> listOfOldPlaces,
    List<FunctionWorldState> functionTemplate,
    out List<FunctionWorldState> newFunction,
    out double minimalValue,
    out WorldState positionOfMinimalValue,
    out double maximalValue,
    out WorldState positionOfMaximalValue,
    Experiment setup)
    {
      maximalValue = Double.MinValue;
      minimalValue = Double.MaxValue;

      positionOfMinimalValue = null;
      positionOfMaximalValue = null;

      newFunction = new List<FunctionWorldState>();
      for (int index = 0; index < functionTemplate.Count; index++)
      {
        FunctionWorldState entry = functionTemplate[index];
        double value = MBLTools.CalculateFunction2(listOfOldPlaces, entry.worldState, setup);
        newFunction.Add(new FunctionWorldState(entry.worldState, value));
        if (value > maximalValue)
        {
          maximalValue = value;
          positionOfMaximalValue = entry.worldState;
        }
        if (value < minimalValue)
        {
          minimalValue = value;
          positionOfMinimalValue = entry.worldState;
        }
      }
    }

    private void DrawFunction(string drawingID,
      List<FunctionWorldState> function, double minimalValue, double maximalValue)
    {
      double rasterWidth = Globals.rasterWidth;
      if (step is WorldState1D) rasterWidth = (step as WorldState1D).Position;
      if (step is WorldState2D) rasterWidth = (step as WorldState2D).Vector.x;
      if (step is WorldStatePose2D) rasterWidth = (step as WorldStatePose2D).Pose.translation.x;

      if (maximalValue - minimalValue == 0) return;
      int counter = 0;
      foreach (FunctionWorldState entry in function)
      {
        int intensity = (int)(Math.Pow((1 - (entry.value - minimalValue) / (maximalValue - minimalValue)), 10) * 200.0);
        entry.worldState.DrawWithIntensity(intensity, drawingID, rasterWidth);
        if (intensity > 198) entry.worldState.DrawShaded(0.7, System.Drawing.Color.Red, drawingID);
        counter++;
      }
    }

    private void ResetRangeAndStepWidth(WorldState positionOfMinimalValue, double refinementFactor)
    {
      if (positionOfMinimalValue is WorldState1D)
      {
        double range = (maximum as WorldState1D).Position - (minimum as WorldState1D).Position;
        range *= refinementFactor;
        (minimum as WorldState1D).Position = (positionOfMinimalValue as WorldState1D).Position - range / 2.0;
        (maximum as WorldState1D).Position = (positionOfMinimalValue as WorldState1D).Position + range / 2.0;
        (step as WorldState1D).Position *= refinementFactor;
      }
      if (positionOfMinimalValue is WorldState2D)
      {
        double rangeX = (maximum as WorldState2D).Vector.x - (minimum as WorldState2D).Vector.x;
        double rangeY = (maximum as WorldState2D).Vector.y - (minimum as WorldState2D).Vector.y;
        rangeX *= refinementFactor;
        rangeY *= refinementFactor;
        (minimum as WorldState2D).Vector.x = (positionOfMinimalValue as WorldState2D).Vector.x - rangeX / 2.0;
        (minimum as WorldState2D).Vector.y = (positionOfMinimalValue as WorldState2D).Vector.y - rangeY / 2.0;
        (maximum as WorldState2D).Vector.x = (positionOfMinimalValue as WorldState2D).Vector.x + rangeX / 2.0;
        (maximum as WorldState2D).Vector.y = (positionOfMinimalValue as WorldState2D).Vector.y + rangeY / 2.0;
        (step as WorldState2D).Vector.x *= refinementFactor;
        (step as WorldState2D).Vector.y *= refinementFactor;
      }
      if (positionOfMinimalValue is WorldStatePose2D)
      {
        double rangeX = (maximum as WorldStatePose2D).Pose.translation.x - (minimum as WorldStatePose2D).Pose.translation.x;
        double rangeY = (maximum as WorldStatePose2D).Pose.translation.y - (minimum as WorldStatePose2D).Pose.translation.y;
        double rangeAngle = (maximum as WorldStatePose2D).Pose.rotation - (minimum as WorldStatePose2D).Pose.rotation;
        rangeX *= refinementFactor;
        rangeY *= refinementFactor;
        rangeAngle *= refinementFactor;
        (minimum as WorldStatePose2D).Pose.translation.x = (positionOfMinimalValue as WorldStatePose2D).Pose.translation.x - rangeX / 2.0;
        (minimum as WorldStatePose2D).Pose.translation.y = (positionOfMinimalValue as WorldStatePose2D).Pose.translation.y - rangeY / 2.0;
        (minimum as WorldStatePose2D).Pose.rotation = (positionOfMinimalValue as WorldStatePose2D).Pose.rotation - rangeAngle / 2.0;

        (maximum as WorldStatePose2D).Pose.translation.x = (positionOfMinimalValue as WorldStatePose2D).Pose.translation.x + rangeX / 2.0;
        (maximum as WorldStatePose2D).Pose.translation.y = (positionOfMinimalValue as WorldStatePose2D).Pose.translation.y + rangeY / 2.0;
        (maximum as WorldStatePose2D).Pose.rotation = (positionOfMinimalValue as WorldStatePose2D).Pose.rotation + rangeAngle / 2.0;
        (step as WorldStatePose2D).Pose.translation.x *= refinementFactor;
        (step as WorldStatePose2D).Pose.translation.y *= refinementFactor;
        (step as WorldStatePose2D).Pose.rotation *= refinementFactor;
      }
    }
  }
}
